/***
 * * PROJECT
 * *    ATaG
 * * VERSION
 * *    $Revision: 1.2 $
 * * DATE
 * *    $Date: 2008/04/19 09:15:52 $
 * * LOG
 * *    $Log: GraphUtils.java,v $
 * *    Revision 1.2  2008/04/19 09:15:52  animesh
 * *    multipath done
 * *
 ***/
package atag.compiler.powerassigner;

import java.util.*;

/**
 *
 * @author Animesh
 */
public class GraphUtils {

    public static final boolean debug = false;
    public static final double INFINITY = 1.0E7;

    /** Creates a new instance of GraphUtils */
    public GraphUtils() {
    }

    public static void main(String[] args) {

        int n = 3;
        int k = 3;
        double[][] s = new double[n][n];

        //fully connected graph
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                s[i][j] = 10.0;
            }
        }

        //connect node to itself, and hope the program handles the 
        //special case well
        for (int i = 0; i < n; i++) {
            s[i][i] = 0;
        }

        double[][][][] R = returnMultiPathRoutingMatrix(s, n, k);

        System.out.println(":::Here are the routing costs:::");
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                for (int m = 0; m < n; m++) {
                    for (int pathID = 0; pathID < k; pathID++) {
                        System.out.print("R["+i+"]["+j+"]["+m+"]["+
                                pathID+"] = "+ R[i][j][m][pathID] + "; ");
                    }
                    System.out.print("\n");
                }
                System.out.print("\n");
            }
            System.out.print("\n");
        }
    }

    /**
     * @returns nodes in a BFS order -- assuming it is a DAG
     * @param s adjacency matrix [0 or non-zero]
     * @param m number of nodes
     */
    public static int[] giveBFSOrder(double[][] s, int m) {
        //array to be returned
        int[] bfs = new int[m];

        //initialize queue to empty
        //LinkedList implements Queue
        Queue<Integer> tempQ = new LinkedList();

        //flag arrays
        boolean[] inTempQ = new boolean[m];
        boolean[] inBFS = new boolean[m];

        //initializing the flag arrays
        for (int i = 0; i < m; i++) {
            inTempQ[i] = false;
            inBFS[i] = false;
        }

        //init the queue with the tasks with no feeders
        for (int i = 0; i < m; i++) {
            boolean isSource = true;
            for (int a = 0; a < m; a++) {
                if (s[a][i] != 0) {
                    //there is a parent to this task
                    isSource = false;
                    break;
                }
            }
            if (isSource) {
                tempQ.offer(new Integer(i));
                inTempQ[i] = true;
            }
        }

        int count = 0;

        Integer intTempI;

        while ((intTempI = tempQ.poll()) != null) {
            int tempi = intTempI.intValue();
            boolean isReady = true;
            for (int i = 0; i < m; i++) {
                if ((s[i][tempi] != 0) && (inBFS[i] == false)) {
                    //there is a feeder who is not in the BFS yet
                    isReady = false;
                    break;
                }
            }//end for
            if (isReady) {
                bfs[count++] = tempi;
                inBFS[tempi] = true;
                inTempQ[tempi] = false;

                for (int j = 0; j < m; j++) {
                    if ((s[tempi][j] != 0) && (inTempQ[j] == false)) {
                        //there is a child task who is not yet in the tempQ
                        tempQ.offer(new Integer(j));
                        inTempQ[j] = true;
                    }
                }
            } else {
                //not ready yet.
                //reinsert the element
                tempQ.offer(intTempI);
            }//end if
        }//end while

        if (count != m) {
            System.out.println("There seems to be an error... not" +
                    " all elements are in the BFS");
            System.exit(3);
        }

        return bfs;
    }

    /**
     * Computes routing costs using Floyd-Warshall
     * @param s edge cost matrix
     * @param n number of nodes
     * @return Matrix R[i][j][k], with entries showing what node k spends when
     * a message is routed from node i to node j. The computation is done using 
     * Floyd-Warshall
     */
    public static double[][][] returnRoutingMatrix(double[][] s, int n) {

        //now move on to R[][][]
        double[][][] R = new double[n][n][n];
        double[][] pathlength = new double[n][n];

        //init the arrays
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                pathlength[i][j] = s[i][j];
                for (int k = 0; k < n; k++) {
                    R[i][j][k] = 0;
                }
                //update for source, i.e., i
                //this can perhaps be multiplied by the cost per transmission at i
                R[i][j][i] = s[i][j];
            }
        }

        //updating R - using floyd warshall
        for (int k = 0; k < n; k++) {
            for (int i = 0; i < n; i++) {
                for (int j = 0; j < n; j++) {
                    //for each pair i,j of nodes
                    if (pathlength[i][j] > (pathlength[i][k] + pathlength[k][j])) {
                        //update with smaller path
                        pathlength[i][j] = pathlength[i][k] + pathlength[k][j];
                        //update R[i][j][tempk] for all nodes tempk
                        for (int tempk = 0; tempk < n; tempk++) {
                            R[i][j][tempk] = R[i][k][tempk] + R[k][j][tempk];
                        }
                    }
                }
            }
        }

        /* change infinity to 0 now -- maybe not.. the infinities 
         * between connected nodes are already non-infinite now.
         * Any infinities at this stage mean that the graph is not connected.
         */
        //TODO handle that case when graph is not connected.

        return R;
    }

    /**
     * Computes Routing cost using Generalized Floyd-Warshall
     * @param s edge cost matrix - uses GraphUtils.INFINITY as infinity cost
     * @param n number of nodes
     * @param k number of paths wanted
     * @return Matrix R[i][j][k][p], with entries showing what node k spends when
     * a message is routed from node i to node j using path p. The computation is done using 
     * Genralized Floyd-Warshall
     */
    public static double[][][][] returnMultiPathRoutingMatrix(double[][] s, int n, int k) {


        /** the d matrix d[i][j][m]. Each item is a k-length vector 
         * in d[i][j][m]: 
         * i: source
         * j: destination
         * m: round
         * d[i][j][m]: The k shortest paths from i to j after m iterations
         * viz, including nodes up to node m in the path
         */
//        KShortestPaths d[][][] = new KShortestPaths[n][n][n + 1];
//        /* initialize d[*][*][0] */
//        /*
//         *  d[i][j][0] = (s[i][j],inf,inf,inf...)
//         */
//
//        for (int i = 0; i < n; i++) {
//            for (int j = 0; j < n; j++) {
//                Path[] newPaths = new Path[k];
//                newPaths[0] = new Path(new int[]{i, j}, s[i][j]);
//
//                /* setting others to infinity */
//                for (int m = 1; m < k; m++) {
//                    newPaths[m] = new Path(new int[]{i, j}, GraphUtils.INFINITY);
//                }
//                d[i][j][0] = new KShortestPaths(i, j, k, newPaths);
//
//            }
//        }
//
//
//        System.err.println("!!!NOT DONE YET!!!");
//        System.err.println("change indices from 1 to n to 0 to n-1");
//        System.exit(-1);
//        /* lets keep these from 1 to n for now.. then change... */
//        for (int m = 1; m < n + 1; m++) {
//
//            d[m][m][m] = convolve(d[m][m][m - 1]);
//
//            for (int i = 1; i < n + 1; i++) {
//                d[i][m][m] = generalAdd(d[i][m][m - 1], d[m][m][m]);
//            }
//
//            for (int j = 1; j < n + 1; j++) {
//                d[m][j][m] = generalAdd(d[m][m][m], d[m][j][m - 1]);
//            }
//
//            for (int i = 1; i < n + 1; i++) {
//                if (i != m) {
//                    for (int j = 1; j < n + 1; j++) {
//                        if (j != m) {
//                            d[i][j][m] = generalMin(
//                                    generalAdd(d[i][m][m], d[m][j][m]),
//                                    d[i][j][m - 1]);
//                        }
//                    }
//                }
//            }
//        }

        //trying to move from a 3D to a 2D d
        KShortestPaths eold[][] = new KShortestPaths[n][n];
        KShortestPaths enew[][] = new KShortestPaths[n][n];

        /* initialize eold[*][*][0] */
        /*
         *  eold[i][j] = (s[i][j],inf,inf,inf...)
         */
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                Path[] newPaths = new Path[k];
                if (s[i][j] == GraphUtils.INFINITY) {
                    //if there is no edge, we do not call it a path
                    newPaths[0] = new Path();
                } else {
                    newPaths[0] = new Path(new int[]{i, j}, s[i][j]);
                }

                /* setting others to infinity */
                for (int m = 1; m < k; m++) {
                    //newPaths[m] = new Path(new int[]{i, j}, GraphUtils.INFINITY);
                    newPaths[m] = new Path();
                }
                eold[i][j] = new KShortestPaths(i, j, k, newPaths);
            }
        }

        //init eold
        for (int m = 0; m < n; m++) {

            if (debug) {
                /*******Debug Print*************/
                //paths gotten. lets print them
                System.out.println("After " + m + " rounds");
                for (int i = 0; i < n; i++) {
                    for (int j = 0; j < n; j++) {
                        System.out.println(eold[i][j].toString());
                    }
                }
            }

            /*******************************/
            //prepare for node m
            enew[m][m] = convolve(eold[m][m]);

            //set for all paths from node i to node m
            for (int i = 0; i < n; i++) {
                if (i != m) {
                    enew[i][m] = generalAdd(eold[i][m], enew[m][m]);
                }
            }

            //set for all paths from node i to node m
            for (int j = 0; j < n; j++) {
                if (j != m) {
                    enew[m][j] = generalAdd(enew[m][m], eold[m][j]);
                }
            }

            //now all paths from i to j
            for (int i = 0; i < n; i++) {
                if (i != m) {
                    for (int j = 0; j < n; j++) {
                        if (j != m) {
                            enew[i][j] = generalMin(
                                    generalAdd(enew[i][m], enew[m][j]),
                                    eold[i][j]);
                        }
                    }
                }
            }

            eold = enew;
            enew = new KShortestPaths[n][n];
        }

        if (debug) {
            //paths gotten. lets print them
            System.out.println("After " + n + " rounds");
            for (int i = 0; i < n; i++) {
                for (int j = 0; j < n; j++) {
                    System.out.println(eold[i][j].toString());
                }
            }
        }


        double[][][][] R = new double[n][n][n][k];
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                for (int m = 0; m < n; m++) {
                    for (int pathID = 0; pathID < k; pathID++) {
                        //init at 0. I don't care if I do not route any data through this
                        R[i][j][m][pathID] = 0.0;
                    }
                }
            }
        }


        for (int i = 0; i < n; i++) {
            //for each source node
            for (int j = 0; j < n; j++) {
                //for each destination node
                //get the set of paths between them
                KShortestPaths thisSetOfPaths = eold[i][j];
                Path[] paths = thisSetOfPaths.getPaths();
                int sourceID = thisSetOfPaths.getSourceID();
                int destinationID = thisSetOfPaths.getDestinationID();
                int numPaths = thisSetOfPaths.getNumpaths();

                //TODO: check that numPaths <= k
                //TODO: check for sourceID and destinationID to match stuff

                for (int rho = 0; rho < numPaths; rho++) {
                    //for each possible path
                    Path thisPath = paths[rho];
                    if (thisPath.getPathLength() != GraphUtils.INFINITY) {
                        int[] nodesInThisPath = thisPath.getNodes();
                        for (int count = 0; count < nodesInThisPath.length-1; count++) {
                            //R[i][j][node][rho] += s[node][nextNodeInPath]
                            //perhaps times costOfSensingPacketFromNode
                            R[i][j][nodesInThisPath[count]][rho] +=
                                    s[nodesInThisPath[count]][nodesInThisPath[count + 1]];
                        }
                    }else{
                        //there is no path #rho between these nodes. Set all 
                        //costs to infinity
                        R[i][j][i][rho] = GraphUtils.INFINITY;
                    }
                }
            }
        }

        return R;
    }

    /**
     * @deprecated
     * @param d
     * @return
     */
    public static ArrayList convolve(ArrayList d) {
        ArrayList retList = new ArrayList(d);

        for (int i = 1; i < d.size(); i++) {
            retList = generalAdd(retList, d, d.size());
        }

        return retList;
    }

    /**
     * Generalized ADD function used for the Floyd algorithm for 
     * all-pairs K-shortest paths.
     * Takes two sorted lists with distinct elements [repeated infinities at end OK], 
     * and returns a merged list of the first k distinct elements in the "sum" of the two,
     * padding the end with infinities if needed.
     * @param a the first list
     * @param b the second list
     * @param size the size of the lists
     * @return the merged array
     * @deprecated
     */
    public static ArrayList generalAdd(ArrayList a, ArrayList b, int size) {
        // create all k^2 items
        int[] sumarray = new int[size * size];
        int sumarrcount = 0;
        for (int indexa = 0; indexa < size; indexa++) {
            int inta = ((Integer) (a.get(indexa))).intValue();
            for (int indexb = 0; indexb < size; indexb++) {
                int intb = ((Integer) (b.get(indexb))).intValue();
                sumarray[sumarrcount++] = inta + intb;
            }
        }

        // execute merge-and-delete-duplicate sort
        int[] paths = new int[size];
        int outarrcount = 0;
        int insize = size * size;

        paths[0] = sumarray[0];
        outarrcount++;

        for (int i = 1; i < insize; i++) {
            if ((outarrcount >= size) &&
                    (sumarray[i] >= paths[outarrcount - 1])) {
                continue;//skip all. move to next i
            } else {
                //the array was not full, or the new item is smaller
                //in any case, [possibly] insert it in the array
                boolean abort = false;//in case this turns out to be duplicate
                //now search for the place to insert at

                int insertAt = outarrcount;
                for (int j = outarrcount - 1; j >= 0; j--) {
                    if (sumarray[i] < paths[j]) {
                        //nothing to see here. move to next j
                        insertAt = j; // useful when new item is smallest
                    } else if (sumarray[i] == paths[j]) {
                        //abort the insertion.. duplicates not allowed
                        abort = true;
                        break;
                    } else {
                        //sumarray[i] > paths[j]
                        //insert at j+1
                        insertAt = j + 1;
                        break;
                    }
                }//end for j

                if (abort) {
                    break; //move to next i
                }

                //increase the upper limit of outarr if you can
                if (outarrcount < size) {
                    outarrcount++;
                }

                // move data from insertat -- outarrcount-2
                // to insertat+1 -- outarrcount-1
                for (int j = outarrcount - 1; j > insertAt; j--) {
                    paths[j] = paths[j - 1];
                }

                paths[insertAt] = sumarray[i];

            }//end else
        }//end for i

        // pad with infinities if needed
        if (outarrcount < size) {
            for (int i = outarrcount; i < size; i++) {
                paths[i] = (int) INFINITY;
            }
        }

        //convert paths to ArrayList of Ints, and return it.
        ArrayList outList = new ArrayList(size);
        for (int i = 0; i < size; i++) {
            outList.add(i, new Integer(paths[i]));
        }

        return outList;

    }

    /**
     * Generalized MIN function used for the Floyd algorithm for 
     * all-pairs K-shortest paths.
     * Takes two sorted lists with distinct elements [repeated infinities at end OK], 
     * and returns a merged list of the first k distinct elements in the two, padding the end with 
     * infinities if needed.
     * @param a the first list
     * @param b the second list
     * @param size the size of the lists
     * @return the merged array
     * @deprecated
     */
    public static ArrayList generalMin(ArrayList a, ArrayList b, int size) {
        ArrayList retList = new ArrayList(size);

        int indexa = 0;
        int indexb = 0;

        for (int count = 0; count < size; count++) {
            //merge a and b
            int inta = ((Integer) (a.get(indexa))).intValue();
            int intb = ((Integer) (b.get(indexb))).intValue();

            if (inta < intb) {
                retList.add(count, new Integer(inta));
                indexa++;
            } else if (inta > intb) {
                retList.add(count, new Integer(intb));
                indexb++;
            } else {
                //inta and intb are equal. We insert only one copy
                retList.add(count, inta); // or intb.. they are same
                indexa++;
                indexb++;
            }
        }

        return retList;

    }

    static KShortestPaths convolve(KShortestPaths d) {
        KShortestPaths retPaths = new KShortestPaths(d);

        for (int i = 1; i < d.getNumpaths(); i++) {
            retPaths = generalAdd(retPaths, d);
        }

        return retPaths;
    }

    /**
     * Generalized ADD function used for the Floyd algorithm for 
     * all-pairs K-shortest paths.
     * Takes two sorted lists with distinct elements [repeated infinities at end OK], 
     * and returns a merged list of the first k distinct elements in the "sum" of the two,
     * padding the end with infinities if needed.
     * @param a the first list of paths
     * @param b the second list of paths
     * @return the merged array
     */
    static KShortestPaths generalAdd(KShortestPaths a, KShortestPaths b) {

        if (a.getDestinationID() != b.getSourceID()) {
            //TODO Some graceful Error code
            System.err.println("Path cannot be added to one if the end of " +
                    "a is not the first node in b.");
            System.err.println("Paths: " + a.getSourceID() + " -> " + a.getDestinationID() +
                    " and " + b.getSourceID() + " -> " + b.getDestinationID());
            System.exit(-1);
        }

        int sourceID = a.getSourceID();
        int destinationID = b.getDestinationID();

        int numpaths = a.getNumpaths(); // or b's numPaths.. same thing
        Path[] paths = new Path[numpaths];

        Path[] aPaths = a.getPaths();
        Path[] bPaths = b.getPaths();

        // create all k^2 items
        Path[] sumarray = new Path[numpaths * numpaths];
        int sumarrcount = 0;
        for (int indexa = 0; indexa < numpaths; indexa++) {
            Path pathFromA = aPaths[indexa];
            for (int indexb = 0; indexb < numpaths; indexb++) {
                Path pathFromB = bPaths[indexb];
                sumarray[sumarrcount++] = Path.addPaths(pathFromA, pathFromB);
            }
        }

        // execute merge-and-delete-duplicate sort
        int outPathArrCount = 0;
        int insize = numpaths * numpaths;

        paths[0] = sumarray[0];
        outPathArrCount++;

        for (int i = 1; i < insize; i++) {
            if ((outPathArrCount >= numpaths) &&
                    (sumarray[i].isLongerThanOrEqualTo(paths[outPathArrCount - 1]))) {
                continue;//skip all. move to next i
            } else {
                //the array was not full, or the new item is smaller
                //in any case, [possibly] insert it in the array
                boolean abort = false;//in case this turns out to be duplicate
                //now search for the place to insert at

                int insertAt = outPathArrCount;
                for (int j = outPathArrCount - 1; j >= 0; j--) {
                    if (sumarray[i].isShorterThan(paths[j])) {
                        //nothing to see here. move to next j
                        insertAt = j; // useful when new item is smallest
                    } else if (sumarray[i].isAsLongAs(paths[j])) {
                        //abort the insertion.. duplicates not allowed
                        abort = true;
                        break;
                    } else {
                        //sumarray[i] > paths[j]
                        //insert at j+1
                        insertAt = j + 1;
                        break;
                    }
                }//end for j

                if (abort) {
                    break; //move to next i
                }

                //increase the upper limit of outarr if you can
                if (outPathArrCount < numpaths) {
                    outPathArrCount++;
                }

                // move data from insertat -- outarrcount-2
                // to insertat+1 -- outarrcount-1
                for (int j = outPathArrCount - 1; j > insertAt; j--) {
                    paths[j] = paths[j - 1];
                }

                paths[insertAt] = sumarray[i];

            }//end else
        }//end for i

        // pad with infinities if needed
        if (outPathArrCount < numpaths) {
            for (int i = outPathArrCount; i < numpaths; i++) {
                paths[i] = new Path();// assures infinity size
            }
        }

        return new KShortestPaths(sourceID, destinationID, numpaths, paths);

    }

    /**
     * Generalized MIN function used for the Floyd algorithm for 
     * all-pairs K-shortest paths.
     * Takes two sorted lists with distinct elements [repeated infinities at end OK], 
     * and returns a merged list of the first k distinct elements in the two, padding the end with 
     * infinities if needed.
     * @param a the first list of paths
     * @param b the second list of paths
     * @return the merged array
     */
    static KShortestPaths generalMin(KShortestPaths a, KShortestPaths b) {

        //TODO Check that the numpaths are the same
        // or at least the sourceIDs, and destinationIDs are the same

        int sourceID = a.getSourceID();
        int destinationID = a.getDestinationID();
        int numpaths = a.getNumpaths();
        Path[] paths = new Path[numpaths];

        Path[] aPaths = a.getPaths();
        Path[] bPaths = b.getPaths();

        int indexa = 0;
        int indexb = 0;

        for (int count = 0; count < numpaths; count++) {
            //merge a and b
            double lengthOfA = aPaths[indexa].getPathLength();
            double lengthOfB = bPaths[indexb].getPathLength();

            if (lengthOfA < lengthOfB) {
                paths[count] = aPaths[indexa].clone();
                indexa++;
            } else if (lengthOfA > lengthOfB) {
                paths[count] = bPaths[indexb].clone();
                indexb++;
            } else {
                //lengthOfA and lengthOfB are equal. We insert only one copy
                paths[count] = aPaths[indexa].clone();// or intb.. they are same
                indexa++;
                indexb++;
            }
        }

        return new KShortestPaths(sourceID, destinationID, numpaths, paths);
    }
}

class KShortestPaths {

    /** the ID of the source node */
    int sourceID;
    /** the ID of the destination node */
    int destinationID;
    /** the number of paths to store */
    int numpaths;
    /** array of arraylists -- the paths
     * these paths are ordered nondecreasing order of distinct path lengths
     * ties broken arbitrarily
     */
    Path[] paths;

    public KShortestPaths(KShortestPaths newpathset) {
        this.sourceID = newpathset.getSourceID();
        this.destinationID = newpathset.getDestinationID();
        this.numpaths = newpathset.getNumpaths();

        this.paths = new Path[this.numpaths];

        Path[] newpaths = newpathset.getPaths();

        //copy the paths into this one
        for (int i = 0; i < this.numpaths; i++) {
            this.paths[i] = newpaths[i].clone();
        }
    }

    public KShortestPaths(int sourceID, int destinationID, int numpaths, Path[] paths) {
        this.sourceID = sourceID;
        this.destinationID = destinationID;
        this.numpaths = numpaths;
        this.paths = paths;
    }

    public int getDestinationID() {
        return destinationID;
    }

    public void setDestinationID(int destinationID) {
        this.destinationID = destinationID;
    }

    public int getNumpaths() {
        return numpaths;
    }

    public void setNumpaths(int numpaths) {
        this.numpaths = numpaths;
    }

    public int getSourceID() {
        return sourceID;
    }

    public void setSourceID(int sourceID) {
        this.sourceID = sourceID;
    }

    public Path[] getPaths() {
        return paths;
    }

    public void setPaths(Path[] paths) {
        this.paths = paths;
    }

    @Override
    public String toString() {
        String retString = new String();
        retString += "paths from node " + this.sourceID + " to node " +
                this.destinationID + " : " + this.numpaths + "\n";
        for (Path p : this.paths) {
            retString += p.toString() + "\n";
        }
        return retString;
    }
}

/** 
 * A path in the graph
 */
class Path {

    /** The node IDs, in order, including source and destination nodes */
    int[] nodes;
    /** The length of the path itself. Set to infinity to denote that 
     * there is no path. This is NOT the number of nodes in the path, but
     * the sum of edge lengths.
     */
    double pathLength;

    /**
     * Create a new path with no nodes and infinite length
     */
    public Path() {
        this.nodes = null;
        this.pathLength = (int) GraphUtils.INFINITY;
    }

    public Path(int[] nodes, double length) {
        this.nodes = nodes;
        this.pathLength = length;
    }

    public double getPathLength() {
        return pathLength;
    }

    public void setPathLength(double length) {
        this.pathLength = length;
    }

    public int[] getNodes() {
        return nodes;
    }

    public void setNodes(int[] nodes) {
        this.nodes = nodes;
    }

    @Override
    /**
     * Returns the string representation of the path. No newlines
     */
    public String toString() {
        if (this.nodes == null) {
            return (new String("Path Does not Exist"));
        } else {
            String retString = new String();
            retString += ":Length : " + this.pathLength + " :  ";
            retString += this.nodes[0];
            for (int i = 1; i < this.nodes.length; i++) {
                retString += " -> " + this.nodes[i];
            }
            retString += ":";
            return retString;
        }
    }

    @Override
    public Path clone() {
        double newlength = this.pathLength;
        int[] newnodes;

        if (this.nodes == null) {
            newnodes = null;
        } else {

            newnodes = new int[this.nodes.length];
            for (int i = 0; i < this.nodes.length; i++) {
                newnodes[i] = this.nodes[i];
            }
        }
        return (new Path(newnodes, newlength));

    }

    /** concatenate paths p1 and p2*/
    static Path addPaths(Path p1, Path p2) {

        //first, take care of the case when one of them is an infinite path
        if ((p1.pathLength >= GraphUtils.INFINITY) ||
                (p2.pathLength >= GraphUtils.INFINITY)) {
            return new Path();
        }

        //note that the paths contain both initial and final nodes
        int[] firstpart = p1.getNodes();
        int[] secondpart = p2.getNodes();

        double newPathLength = p1.getPathLength() + p2.getPathLength();


        //check if the second path begins with what the first end with.. 
        if (firstpart[firstpart.length - 1] != secondpart[0]) {
            //TODO Some graceful Error code
            System.err.println("Path cannot be added to one if the end of " +
                    "p1 ("+ firstpart[0] + " -> " + 
                    firstpart[firstpart.length-1] + ")is not the first " +
                    "node in p2 ("+ secondpart[0] + " -> " + 
                    secondpart[secondpart.length-1] + ")");
//            System.err.println("Paths: " + a.getSourceID() + " -> " + a.getDestinationID() +
//                    " and " + b.getSourceID() + " -> " + b.getDestinationID() );
            System.exit(-1);
        }

        
        //what if one of them is just a single edge from a node to itself?
        if((firstpart.length == 2) && (firstpart[0] == firstpart[1])){
            //copy secondpart into newnodes. The firstpart is moot
            int newnodes[] = new int[secondpart.length];
            for (int i = 0; i < secondpart.length; i++) {
                newnodes[i] = secondpart[i];
            }
            //update the length and nodes
            return new Path(newnodes, newPathLength);
        }

        if((secondpart.length == 2) && (secondpart[0] == secondpart[1])){
            //copy firstpart into newnodes. The secondpartpart is moot
            int[] newnodes = new int[firstpart.length];
            for (int i = 0; i < firstpart.length; i++) {
                newnodes[i] = firstpart[i];
            }
            //update the length and nodes
            return new Path(newnodes, newPathLength);
        }
        
        
        
        //no errors, proceed to merge
        //the middle node is repeated.. hence the -1
        int[] newnodes = new int[firstpart.length + secondpart.length - 1];
        int newindex = 0;
        for (int i = 0; i < firstpart.length; i++, newindex++) {
            newnodes[newindex] = firstpart[i];
        }

        //note how j begins with 1
        for (int j = 1; j < secondpart.length; j++, newindex++) {
            newnodes[newindex] = secondpart[j];
        }

        //update the length and nodes
        return new Path(newnodes, newPathLength);
    }

    /**
     * 
     * @param p The other path
     * @return true if this is shorter than p2. Ties broken in favor of this
     */
    public boolean isShorterThanOrEqualTo(Path p) {
        return (this.pathLength <= p.getPathLength());
    }

    /**
     * 
     * @param p The other path
     */
    public boolean isLongerThanOrEqualTo(Path p) {
        return (this.pathLength >= p.getPathLength());
    }

    /**
     * 
     * @param p The other path
     */
    public boolean isLongerThan(Path p) {
        return (this.pathLength > p.getPathLength());
    }

    /**
     * 
     * @param p The other path
     */
    public boolean isShorterThan(Path p) {
        return (this.pathLength < p.getPathLength());
    }

    /**
     * 
     * @param p The other path
     */
    public boolean isAsLongAs(Path p) {
        return (this.pathLength == p.getPathLength());
    }

    /**
     * @param p1 first path
     * @param p2 second path
     * @return the path with the shorter length. Ties broken in favor of p1
     */
    public static Path returnShorter(Path p1, Path p2) {
        if (p1.getPathLength() > p2.getPathLength()) {
            return p2;
        } else {
            return p1;
        }
    }
}
